import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, PoseArray, Point32
from std_msgs.msg import Float32MultiArray, Int8, Float32
from PyQt5.QtCore import pyqtSignal, QObject
from trace_visualizer_py.common import common_data

def ros_print(str_data):
    print('[ROS2]: ' + str_data, end='')

class RosNode(Node, QObject):
    sig_new_point = pyqtSignal(float, float)
    sig_new_uwb_pos = pyqtSignal(float, float, float, float, float, float)


    def __init__(self):
        Node.__init__(self, 'qt_node')
        QObject.__init__(self)
        self.sig_new_point.connect(common_data['widget_trace'].add_point)
        self.sig_new_uwb_pos.connect(lambda x1,y1,x2,y2,x3,y3: (common_data['widget_trace'].update_uwb_location(x1,y1,x2,y2,x3,y3), 
                                                                common_data['cfg_basic'].set("UWB1_pos", [x1, y1]),
                                                                common_data['cfg_basic'].set("UWB2_pos", [x2, y2]),
                                                                common_data['cfg_basic'].set("UWB3_pos", [x3, y3])
                                                                )
                                    )


        # 订阅话题，接收到消息后发送信号
        self.ship_pos_sub = self.create_subscription(
            Point32,        # 替换为实际的消息类型
            '/ship/position',   # 替换为实际的话题名称
            self.ship_pos_callback,
            10
        )
        self.ship_pose_sub = self.create_subscription(
            PoseStamped,    # 替换为实际的消息类型
            '/ship/pose',   # 替换为实际的话题名称
            self.ship_pose_callback,
            10
        )

        self.ship_motor_sub = self.create_subscription(
            PoseStamped,                # 替换为实际的消息类型
            '/ship/motor_velocity',     # 替换为实际的话题名称
            self.ship_pose_callback,
            10
        )
        self.ship_motor_sub = self.create_subscription(
            Float32MultiArray,                # 替换为实际的消息类型
            '/uwb/position',     # 替换为实际的话题名称
            self.uwb_anchor_pos_callback,
            10
        )

        self.trace_pub = self.create_publisher(PoseArray, '/ship/data', 10)
        self.uwb_mode_pub = self.create_publisher(Int8, '/ui/uwb_cmd', 10)


    def ship_pos_callback(self, msg):
        # 从消息中提取 x 和 y 坐标
        x = msg.x
        y = msg.y
        self.sig_new_point.emit(x, y)

    def ship_pose_callback(self, msg):
        pass

    def ship_motor_callback(self, msg):
        pass

    def uwb_anchor_pos_callback(self, msg):
        uwb_data = msg.data
        x1, y1 = uwb_data[0], uwb_data[1]
        x2, y2 = uwb_data[2], uwb_data[3]
        x3, y3 = uwb_data[4], uwb_data[5]
        self.sig_new_uwb_pos.emit(x1, y1, x2, y2, x3, y3)

    def publish_trace(self, points):
        """发布轨迹点"""
        pose_array = PoseArray()
        pose_array.header.stamp = self.get_clock().now().to_msg()
        pose_array.header.frame_id = 'map'  # 替换为实际的坐标系

        for point in points:
            pose = PoseStamped().pose
            pose.position.x = point.x()
            pose.position.y = point.y()
            pose.position.z = 0.0
            pose.orientation.x = 0.0
            pose.orientation.y = 0.0
            pose.orientation.z = 0.0
            pose.orientation.w = 1.0
            pose_array.poses.append(pose)

        self.trace_pub.publish(pose_array)
        ros_print('Published trajectory with {} points'.format(len(points)))

    def publish_uwb_mode(self, mode):
        cmd = Int8()
        cmd.data = mode
        self.uwb_mode_pub.publish(cmd)
        ros_print(f'Published UWB cmd: {"$ancrangestart" if mode==1 else "$ancrangestop"}')
    
common_data['node'] = RosNode()